/**
 *******************************************************************************
 * @FileName  : svc_disc.c
 * @Author    : GaoQiu
 * @CreateDate: 2020-05-18
 * @Copyright : Copyright(C) GaoQiu
 *              All Rights Reserved.
 *******************************************************************************
 *
 * The information contained herein is confidential and proprietary property of
 * GaoQiu and is available under the terms of Commercial License Agreement
 * between GaoQiu and the licensee in separate contract or the terms described
 * here-in.
 *
 * This heading MUST NOT be removed from this file.
 *
 * Licensees are granted free, non-transferable use of the information in this
 * file under Mutual Non-Disclosure Agreement. NO WARRENTY of ANY KIND is provided.
 *
 *******************************************************************************
 */
#include "svc_disc.h"
#include "utils/assert.h"
#include "utils/lg_mem.h"
#include "utils/byte_stream.h"

#include "stack/host/att_uuid.h"
#include "stack/host/att_api.h"
#include "stack/host/dm_api.h"

static MemPool_t svrPool;
static MemPool_t chrPool;
static MemPool_t descPool;
static MemPool_t connPool;

static queue_t sdpCbList;

void SDP_DiscAllChrs(SdpCb_t *pSdpCb);
void SDP_DiscAllCCCD(SdpCb_t *pSdpCb);
int SDP_DiscChrCb(uint16_t connHandle, attDiscState_t state, attChr_t *pChr, void *arg);
int SDP_DiscDscCb(uint16_t connHandle, attDiscState_t state, attDsc_t *pDsc, void *arg);
int SDP_SetCccdCb(uint16_t connHandle, attDiscState_t state, void *arg);

// connPool->svrPool->chrPool->descPool
void SDP_Init(uint8_t numMaxConn, uint8_t numMaxSvc, uint8_t numMaxChr, uint8_t numMaxDesc)
{
	uint8_t *pBuf = lg_mem_new(MEM_ALIGNED4(sizeof(Svr_t)) * numMaxSvc);
	ASSERT(pBuf != NULL, 0);
	MemPoolInit(&svrPool, pBuf, MEM_ALIGNED4(sizeof(Svr_t)), numMaxSvc);

	pBuf = lg_mem_new(MEM_ALIGNED4(sizeof(Chr_t)) * numMaxChr);
	ASSERT(pBuf != NULL, 0);
	MemPoolInit(&chrPool, pBuf, MEM_ALIGNED4(sizeof(Chr_t)), numMaxChr);

	pBuf = lg_mem_new(MEM_ALIGNED4(sizeof(Desc_t)) * numMaxDesc);
	ASSERT(pBuf != NULL, 0);
	MemPoolInit(&descPool, pBuf, MEM_ALIGNED4(sizeof(Desc_t)), numMaxDesc);

	pBuf = lg_mem_new(MEM_ALIGNED4(sizeof(SdpCb_t)) * numMaxConn);
	ASSERT(pBuf != NULL, 0);
	MemPoolInit(&connPool, pBuf, MEM_ALIGNED4(sizeof(SdpCb_t)), numMaxConn);

	QUEUE_INIT(&sdpCbList);
}

SdpCb_t* SDP_Find(uint16_t connHandle)
{
	SdpCb_t *pSdp = sdpCbList.pHead;

	while(pSdp != NULL){
		if(pSdp->connHandle == connHandle){
			return pSdp;
		}

		pSdp = pSdp->pNext;
	}

	return NULL;
}

int SDP_Add(uint16_t connHandle)
{
	SdpCb_t *pSdp = SDP_Find(connHandle);
	if(pSdp){
		return -1;
	}

	pSdp = MemPoolAlloc(&connPool, sizeof(SdpCb_t));
	if(pSdp == NULL){
		return -2;
	}

	pSdp->connHandle = connHandle;
	pSdp->pNext  = NULL;
	pSdp->curSvr = NULL;
	pSdp->curChr = NULL;
	pSdp->SdpCmplCback = NULL;
	QUEUE_INIT(&pSdp->svrList);

	QUEUE_Enquene(&sdpCbList, pSdp);

	SDP_TRACK_INFO("Add Connection Ok(connHandle:%d)...\r\n",connHandle);
	return 0;
}

void SDP_DeleteChr(Chr_t *chr)
{
	Desc_t *pDesc = NULL;
	while((pDesc = QUEUE_Dequeue(&chr->desList))){
		MemPoolFree(&descPool, pDesc);
	}

	MemPoolFree(&chrPool, chr);
}

void SDP_DeleteSvc(Svr_t *svc)
{
	Chr_t *chr = NULL;
	while((chr = QUEUE_Dequeue(&svc->chrList))){
		SDP_DeleteChr(chr);
	}

	MemPoolFree(&svrPool, svc);
}

int SDP_Delete(uint16_t connHandle)
{
	SdpCb_t *pSdp = SDP_Find(connHandle);
	if(pSdp == NULL){
		return -1;
	}

	Svr_t *pSvr = NULL;
	while((pSvr = (Svr_t *)QUEUE_Dequeue(&pSdp->svrList))){
		SDP_DeleteSvc(pSvr);
	}

	SdpCb_t *pPrev = NULL;
	SdpCb_t *pCur = sdpCbList.pHead;
	if(pCur == pSdp){
		QUEUE_RemoveElement(&sdpCbList, pCur, NULL);
	}
	else{
		while(pCur)
		{
			pPrev = pCur;
			pCur = pCur->pNext;
			if(pCur == pSdp)
			{
				QUEUE_RemoveElement(&sdpCbList, pCur, pPrev);
			}
		}
	}
	MemPoolFree(&connPool, pSdp);

	return 0;
}

int SDP_AddSvr(SdpCb_t *pSdp , attSvc_t *pSvrItem)
{
	if(pSdp == NULL){
		return -1;
	}

	Svr_t *pSvr = MemPoolAlloc(&svrPool, 0);
	if(pSvr){
		pSvr->svc = *pSvrItem;
		pSvr->pNext = NULL;
		QUEUE_INIT(&pSvr->chrList);
		QUEUE_Enquene(&pSdp->svrList, pSvr);

		//SDP_TRACK_INFO("Add Svr Ok...\r\n");
	}
	else{
		SDP_TRACK_ERR("Add Svr failed; allocate buffer failed!\r\n");
		return -1;
	}
	return 0;
}

int SDP_AddChr(Svr_t *pSvr, attChr_t *pChrItem)
{
	Chr_t *pChr = MemPoolAlloc(&chrPool, 0);
	if(pChr)
	{
		pChr->chr = *pChrItem;
		pChr->pNext= NULL;
		QUEUE_INIT(&pChr->desList);
		QUEUE_Enquene(&pSvr->chrList, pChr);
	}
	else{
		SDP_TRACK_ERR("Add Chr failed; allocate buffer failed!\r\n");
	}
	return 0;
}

int SDP_AddDesc(Chr_t *pChr, attDsc_t *pDescItem)
{
	Desc_t *pDesc = MemPoolAlloc(&descPool, 0);
	if(pDesc)
	{
		pDesc->dsc = *pDescItem;
		pDesc->pNext = NULL;
		QUEUE_Enquene(&pChr->desList, pDesc);
		//SDP_TRACK_INFO("Add Desc OK\r\n");
	}
	else{
		SDP_TRACK_ERR("Add Desc failed; allocate buffer failed!\r\n");
	}
	return 0;
}

void SDP_DiscCmpl(SdpCb_t *pSdpCb, uint8_t state)
{
	SDP_TRACK_INFO("=== SDP Finished(res: %d) ===\r\n", state);

	if(pSdpCb->SdpCmplCback){
		pSdpCb->SdpCmplCback(pSdpCb->connHandle, state);
	}
}

Desc_t *SDP_GetCccd(Chr_t *pChr)
{
	if(pChr == NULL)
		return NULL;

	Desc_t *pDsc = NULL;

	if(pChr->chr.prop & (ATT_PROP_NOTIFY | ATT_PROP_INDICATE)) {
		pDsc = pChr->desList.pHead;
		return pDsc;
	}
	return NULL;
}

Desc_t *SDP_GetCccdEx(Chr_t *pChr, Chr_t **pDstChr)
{
	if(pChr == NULL){
		if(pDstChr) *pDstChr = NULL;
		return NULL;
	}

	Desc_t *pDsc = NULL;

	while(pChr){
		if(pChr->chr.prop & (ATT_PROP_NOTIFY | ATT_PROP_INDICATE)) {
			pDsc = pChr->desList.pHead;
			if(pDstChr) *pDstChr = pChr;
			return pDsc;
		}
		pChr = pChr->pNext;
	}
	if(pDstChr) *pDstChr = NULL;
	return NULL;
}

void SDP_SetAllCccd(SdpCb_t *pSdpCb)
{
	Svr_t *pSvr = pSdpCb->curSvr;
	Chr_t *pChr = pSdpCb->curChr;
	Desc_t *pDsc = pSdpCb->curDsc;

	uint16_t value = 0x0000;

	if(pDsc == NULL)
	{
		if(pChr) { pChr = pChr->pNext; }
		pDsc = SDP_GetCccdEx(pChr, &pChr);
		if(pChr == NULL){
			pSvr = pSvr->pNext;
			while(pSvr)
			{
				pChr = pSvr->chrList.pHead;
				if((pDsc = SDP_GetCccdEx(pChr, &pChr))){
					break;
				}
				pSvr = pSvr->pNext;
			}
		}
	}

	if(pSvr == NULL || pChr == NULL || pDsc == NULL){
		SDP_DiscCmpl(pSdpCb, 0);
		return;
	}

	pSdpCb->curSvr = pSvr;
	pSdpCb->curChr = pChr;
	pSdpCb->curDsc = pDsc;

	value = pChr->chr.prop & ATT_PROP_NOTIFY ? NOTIFY_ENABLE:INDICATE_ENABLE;

	/* Set Next CCCD */
	int rc = ATTC_SetCCCD(pSdpCb->connHandle, pDsc->dsc.handle, value, SDP_SetCccdCb, pSdpCb);
	if(rc != BLE_OK){
		SDP_DiscCmpl(pSdpCb, 0);
	}
}

int SDP_SetCccdCb(uint16_t connHandle, attDiscState_t state, void *arg)
{
	SdpCb_t *pSdpCb = arg;
	ASSERT(pSdpCb != NULL, 0);

	int rc = BLE_OK;

	switch(state.status)
	{
	case BLE_OK:
		break;

	case BLE_SDP_DONE:
		SDP_TRACK_INFO("CCCD Set OK: %d\n", pSdpCb->curDsc->dsc.handle);
		pSdpCb->curDsc = pSdpCb->curDsc->pNext;
		SDP_SetAllCccd(pSdpCb);
		break;

	default:
		rc = state.status;
		break;
	}

	return rc;
}

Chr_t *SDP_FindContainCccdChr(Chr_t *chr)
{
	if(chr == NULL)
		return NULL;

	while(chr)
	{
		if(chr->chr.prop & (ATT_PROP_NOTIFY|ATT_PROP_INDICATE)){
			return chr;
		}
		chr = chr->pNext;
	}
	return NULL;
}

void SDP_DiscAllCCCD(SdpCb_t *pSdpCb)
{
	ASSERT(pSdpCb != NULL, 0);

	Svr_t *pSvr = pSdpCb->curSvr;
	Chr_t *pChr = pSdpCb->curChr;

	pChr = SDP_FindContainCccdChr(pChr);

	if(pChr == NULL)
	{
		if(pSvr) pSvr = pSvr->pNext;
		while(pSvr)
		{
			pChr = pSvr->chrList.pHead;
			//if(pChr != NULL){
			//	break;
			//}
			pChr = SDP_FindContainCccdChr(pChr);
			if(pChr) { break; }
			pSvr = pSvr->pNext;
		}
	}

	if(pSvr == NULL || pChr == NULL){
	#if 0
		SDP_DiscCmpl(pSdpCb, 0);
	#else
		SDP_TRACK_WRN("Start CCCD Set\n");
		pSdpCb->curSvr = pSdpCb->svrList.pHead;
		pSdpCb->curChr = pSdpCb->curSvr->chrList.pHead;
		pSdpCb->curDsc = NULL;
		SDP_SetAllCccd(pSdpCb);
	#endif
		return;
	}

	pSdpCb->curSvr = pSvr;
	pSdpCb->curChr = pChr;

	/* Find Next CCCD */
	uint16_t endHandle = pSvr->svc.endHandle;
	if(pChr->pNext != NULL){
		endHandle = pChr->pNext->chr.defHandle;
	}

	int rc = ATTC_DiscAllCCCD(pSdpCb->connHandle, pChr->chr.valHandle, endHandle, SDP_DiscDscCb, pSdpCb);
	if(rc != BLE_OK){
		SDP_DiscCmpl(pSdpCb, rc);
	}
}

int SDP_DiscDscCb(uint16_t connHandle, attDiscState_t state, attDsc_t *pDsc, void *arg)
{
	SdpCb_t *pSdp = arg;
	ASSERT(pSdp != NULL, 0);

	int rc = BLE_OK;

	switch(state.status)
	{
	case BLE_OK:
		SDP_TRACK_INFO("** CCCD Handle: %d\n", pDsc->handle);
		rc = SDP_AddDesc(pSdp->curChr, pDsc);
		break;

	case BLE_SDP_DONE:
		pSdp->curChr = pSdp->curChr->pNext;
		SDP_DiscAllCCCD(pSdp);
		rc = BLE_OK;//[!!! important]
		break;

	default:
		rc = state.status;
		break;
	}

	return rc;
}

void SDP_DiscAllChrs(SdpCb_t *pSdpCb)
{
	Svr_t *pSvr = pSdpCb->curSvr;

	/* Descriptor Discovery */
	if(pSvr == NULL){
		SDP_TRACK_WRN("Start CCCD Find...\r\n");
		pSdpCb->curSvr = pSdpCb->svrList.pHead;
		pSdpCb->curChr = pSdpCb->curSvr->chrList.pHead;
		SDP_DiscAllCCCD(pSdpCb);
		return;
	}

	/* Find Chrs of Next Svc  */
	int rc = ATTC_DiscAllChrs(pSdpCb->connHandle, pSvr->svc.startHandle, pSvr->svc.endHandle, SDP_DiscChrCb, pSdpCb);
	if(rc != BLE_OK){
		SDP_DiscCmpl(pSdpCb, rc);
	}
}

int SDP_DiscChrCb(uint16_t connHandle, attDiscState_t state, attChr_t *pChr, void *arg)
{
	SdpCb_t *pSdp = arg;
	ASSERT(pSdp != NULL, 0);

	int rc = BLE_OK;

	switch(state.status)
	{
	case BLE_OK:
		SDP_TRACK_INFO("** Chr valHandle: %d\n", pChr->valHandle);
		rc = SDP_AddChr(pSdp->curSvr, pChr);
		break;

	case BLE_SDP_DONE:
		pSdp->curSvr = pSdp->curSvr->pNext;
		SDP_DiscAllChrs(pSdp);
		rc = BLE_OK;//[!!! important]
		break;

	default:
		rc = state.status;
		break;
	}

	if(rc != BLE_OK){
		SDP_DiscCmpl(pSdp, rc);
	}
	return rc;
}

int SDP_DiscSvcCb(uint16_t connHandle, attDiscState_t state, attSvc_t *pSvr, void *arg)
{
	int rc = BLE_OK;

	SdpCb_t *pSdp = arg;
	ASSERT(pSdp != NULL, 0);

	switch(state.status)
	{
	case BLE_OK:
		SDP_TRACK_INFO("** Service Handle:(s:%d e:%d)\n", pSvr->startHandle, pSvr->endHandle);
		rc = SDP_AddSvr(pSdp , pSvr);
		break;

	case BLE_SDP_DONE:
		if(1){
			SDP_TRACK_WRN("Start Char Find...\r\n");
			pSdp->curSvr = pSdp->svrList.pHead;
			SDP_DiscAllChrs(pSdp);
		}
		rc = BLE_OK;//[!!! important]
		break;

	default:
		rc = state.status;
		break;
	}

	if(rc != BLE_OK){
		SDP_DiscCmpl(pSdp, rc);
	}

	return rc;
}

void SDP_DiscAll(uint16_t connHandle, SdpCmplCback_t func)
{
	SdpCb_t *pSdp = SDP_Find(connHandle);
	if(pSdp == NULL){
		return;
	}

	pSdp->SdpCmplCback = func;

	SDP_TRACK_INFO("=== SDP Start ===\r\n");
	SDP_TRACK_WRN("Start Service Find...\r\n");

	int rc = ATTC_DiscAllSvcs(connHandle, SDP_DiscSvcCb, pSdp);
	if(rc != BLE_OK){
		SDP_TRACK_ERR("SDP Start Failed - res =%d\n", rc);
	}
}

Svr_t *SDP_FindSvrByHandle(uint16_t connHandle, uint16_t attHandle)
{
	SdpCb_t *pSdpCb = SDP_Find(connHandle);
	if(pSdpCb == NULL){
		return NULL;
	}

	Svr_t *pSvr = pSdpCb->svrList.pHead;

	while(pSvr)
	{
		if(pSvr->svc.startHandle == attHandle){
			return pSvr;
		}
		pSvr = pSvr->pNext;
	}

	return NULL;
}

Svr_t *SDP_FindSvrByUuid(uint16_t connHandle, void *uuid, uint32_t uuidLen)
{
	SdpCb_t *pSdpCb = SDP_Find(connHandle);
	if(pSdpCb == NULL){
		return NULL;
	}

	Svr_t *pSvr = pSdpCb->svrList.pHead;
	while(pSvr)
	{
		if(memcmp(pSvr->svc.uuid.uuid_128, uuid, uuidLen) == 0){
			return pSvr;
		}
		pSvr = pSvr->pNext;
	}
	return NULL;
}

Chr_t *SDP_FindCharByHandle(uint16_t connHandle, uint16_t attHandle)
{
	SdpCb_t *pSdpCb = SDP_Find(connHandle);
	if(pSdpCb == NULL){
		return NULL;
	}

	Svr_t *pSvr = pSdpCb->svrList.pHead;
	while(pSvr)
	{
		Chr_t *pChr = pSvr->chrList.pHead;
		while(pChr)
		{
			if(pChr->chr.valHandle == attHandle){
				return pChr;
			}

			pChr = pChr->pNext;
		}

		pSvr = pSvr->pNext;
	}
	return NULL;
}

Chr_t *SDP_FindCharByUUID(uint16_t connHandle, uint8_t *pUuid, uint32_t len)
{
	SdpCb_t *pSdpCb = SDP_Find(connHandle);
	if(pSdpCb == NULL){
		return NULL;
	}

	Svr_t *pSvr = pSdpCb->svrList.pHead;
	while(pSvr)
	{
		Chr_t *pChr = pSvr->chrList.pHead;
		while(pChr)
		{
			if(memcmp(pChr->chr.uuid.uuid_128, pUuid, len) == 0){
				return pChr;
			}
			pChr = pChr->pNext;
		}
		pSvr = pSvr->pNext;
	}
	return NULL;
}


